/*
 * clock.c
 *
 *  Created on: Oct 9, 2023
 *      Author: Sx107
 */


#include <drv/clock.h>

void system_init() {
	MODIFY_REG(FLASH->ACR, FLASH_ACR_LATENCY, FLASH_ACR_LATENCY_4WS);
	WAITVAL(FLASH->ACR, FLASH_ACR_LATENCY, FLASH_ACR_LATENCY_4WS);
	NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);

	MODIFY_REG(PWR->CR3, (PWR_CR3_SCUEN | PWR_CR3_LDOEN | PWR_CR3_BYPASS), PWR_CR3_LDOEN);
	#if defined (PWR_CPUCR_PDDS_D2)
		MODIFY_REG(PWR->D3CR, PWR_D3CR_VOS, 0b11 << PWR_D3CR_VOS_Pos);
	#else
		#error "Voltage scaling not set!"
		//MODIFY_REG(PWR->SRDCR, PWR_SRDCR_VOS, VoltageScaling);
	#endif /* PWR_CPUCR_PDDS_D2 */

	RCC->CR |= RCC_CR_HSEON;
	WAITBIT(RCC->CR, RCC_CR_HSERDY);

	RCC->CR |= RCC_CR_HSION;
	WAITBIT(RCC->CR, RCC_CR_HSIRDY);

	MODIFY_REG(RCC->PLLCKSELR, RCC_PLLCKSELR_PLLSRC, RCC_PLLCKSELR_PLLSRC_HSE);

	RCC->PLLCFGR |= RCC_PLLCFGR_DIVP1EN | RCC_PLLCFGR_DIVR1EN | RCC_PLLCFGR_DIVQ1EN;
	MODIFY_REG(RCC->PLLCFGR, RCC_PLLCFGR_PLL1RGE, 0b11 << RCC_PLLCFGR_PLL1RGE_Pos);			// VCO Input range 8-16 MHz for 0b11, p. 401
	MODIFY_REG(RCC->PLLCFGR, RCC_PLLCFGR_PLL1VCOSEL, 0b0 << RCC_PLLCFGR_PLL1VCOSEL_Pos);	// Wide VCO range: 192 to 960 MHz, p. 401

	// 480Mhz from 24Mhz
	MODIFY_REG(RCC->PLLCKSELR, RCC_PLLCKSELR_DIVM1, 2 << RCC_PLLCKSELR_DIVM1_Pos);	// M /2
	MODIFY_REG(RCC->PLL1DIVR, RCC_PLL1DIVR_N1, (76-1UL) << RCC_PLL1DIVR_N1_Pos);   	// N 80
	MODIFY_REG(RCC->PLL1DIVR, RCC_PLL1DIVR_P1, (2-1UL) << RCC_PLL1DIVR_P1_Pos);		// P 2
	MODIFY_REG(RCC->PLL1DIVR, RCC_PLL1DIVR_Q1, (12-1UL) << RCC_PLL1DIVR_Q1_Pos);	// Q 12 (For QSPI)
	MODIFY_REG(RCC->PLL1DIVR, RCC_PLL1DIVR_R1, (2-1UL) << RCC_PLL1DIVR_R1_Pos);		// R 2
	__DSB();

	RCC->CR |= RCC_CR_PLL1ON;
	WAITBIT(RCC->CR, RCC_CR_PLL1RDY);

	// Set HPRE to /2
	#if defined(RCC_D1CFGR_HPRE)
		MODIFY_REG(RCC->D1CFGR, RCC_D1CFGR_HPRE, RCC_D1CFGR_HPRE_DIV2);
	#else
		#error "RCC_D1CFGR_HPRE Not set!"
	#endif /* RCC_D1CFGR_HPRE */

	MODIFY_REG(RCC->CFGR, RCC_CFGR_SW, RCC_CFGR_SW_PLL1);
	MODIFY_REG(RCC->D1CFGR, RCC_D1CFGR_D1CPRE, RCC_D1CFGR_D1CPRE_DIV1); // Systick prescaler
	MODIFY_REG(RCC->D2CFGR, RCC_D2CFGR_D2PPRE1, RCC_D2CFGR_D2PPRE1_DIV2);
	MODIFY_REG(RCC->D2CFGR, RCC_D2CFGR_D2PPRE2, RCC_D2CFGR_D2PPRE2_DIV2);
	MODIFY_REG(RCC->D1CFGR, RCC_D1CFGR_D1PPRE, RCC_D1CFGR_D1PPRE_DIV2);
	MODIFY_REG(RCC->D3CFGR, RCC_D3CFGR_D3PPRE, RCC_D3CFGR_D3PPRE_DIV2);

	// Systick init
	__disable_irq();
	SysTick->LOAD = (uint32_t)(480000 + 1);
	SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk;
	SysTick->VAL = 0UL;
	__enable_irq();
}

void _delayMs(uint32_t del) {
	SysTick->VAL = 0;
	while (del) {
		if ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0U)
		{
			del--;
	    }
	}
}

void usb_clock_init() {
	RCC->CR |= RCC_CR_HSI48ON;
	WAITBIT(RCC->CR, RCC_CR_HSI48RDY);

	RCC->AHB4ENR |= RCC_AHB4ENR_GPIOAEN;
	MODIFY_REG(GPIOA->MODER, GPIO_MODER_MODE8_Msk, 0b10 << GPIO_MODER_MODE8_Pos);
	MODIFY_REG(GPIOA->OSPEEDR, GPIO_OSPEEDR_OSPEED8_Msk, 0b11 << GPIO_OSPEEDR_OSPEED8_Pos);
	MODIFY_REG(GPIOA->AFR[1], GPIO_AFRH_AFSEL8_Msk, 0 << GPIO_AFRH_AFSEL8_Pos);

	MODIFY_REG(RCC->CFGR, (RCC_CFGR_MCO1 | RCC_CFGR_MCO1PRE), ((0b010 << RCC_CFGR_MCO1_Pos) | (1 << RCC_CFGR_MCO1PRE_Pos)));
}
